///////////////////////////////////////////////////////////////////////////////
// Name:        localhistintegral.cpp
// Purpose:     Local integral histogram
// Copyright:   (c) 2006, Ivan Laptev, TT Solutions
// License:     GPL, Please contact us for commercial license.
//              http://www.tt-solutions.com/
///////////////////////////////////////////////////////////////////////////////
#include "objectdet/localhistintegral.h"

/**
 * Add and fill in some cells into the feature vector.
 * Add exactly 'channels' cells to the feature and compute
 * the integral of the subwindow of imgproc (along the X and Y axis).
 * Imgproc is already an integral histogram so use subwindow corner values to
 * compute it in O(1).
 * For any matrix V and its integral matrix I:
 * Summ({V_ij}, x1 <= i < x2, y1 <= j < y2) = I(x2,y2) - I(x1,y2) - I(x2,y1) + I(x1,y1)
 */
inline
void localhistintegralsub(arrMat integral_block_histogram,    // <- input
                          size_t channels,                        // <- input
                          size_t p1, size_t p2, size_t p3, size_t p4,   // <- input
                          bool normflag,
                          std::vector<double> & feature)        // <- output
{
    size_t const feature_oldsize = feature.size();

//    feature.resize(feature_oldsize + channels, 0.);

    /**
     * We also need to normalize the newly added values,
     * therefore compute the summ in 'total' when filling the feature
     */
    float total = 0.;
    for (size_t i = 0; i < channels; ++i)
    {
        float const * first = (float*)(integral_block_histogram[i]->data.ptr);
        float const value = *(first + p1)
                          + *(first + p4)
                          - *(first + p2)
                          - *(first + p3);
        total += value;
        feature.push_back(value);
    }

    /**
     * And now normalize (if needed) by the 'total' summ
     */
    if (normflag)
    {
        if (total == 0)
            total = 1;
        typedef std::vector<double>::reverse_iterator fiter;
        fiter fi = feature.rbegin();
        /**
         * 'i' isn't used inside the loop. It is used as a counter for the loop number.
         * The actual variable is fi which is initialized to the feature last element.
         */
        for (size_t i = 0; i < channels; ++i)
        {
            *(fi++) /= total;
        }
    }
}

void localhistintegral(StageRule const & rule,                          // <- input
                       arrMat integral_block_histogram,               // <- input
                       size_t channels,
                       size_t left, size_t top, size_t width, size_t height,    // <- input
                       std::vector<double> & feature)                   // <- output
{
//    size_t const channels = cvGetDimSize(imgproc, 2);
//    bool const normflag = (nargin < 7);
    bool const normflag = true;

    size_t w;
    size_t h;
    size_t corner_1 = (top + rule.top)*integral_block_histogram[0]->cols + left + rule.left;
    size_t corner_1to2_shift;
    size_t corner_1to3_shift;
    size_t corner_1to4_shift;

    if (!rule.cached)
    {
        w = rule.width;
        h = rule.height;
        if (rule.type == SrtTwoCols || rule.type == SrtFourQuarters)
            w /= 2;
        if (rule.type == SrtTwoRows || rule.type == SrtFourQuarters)
            h /= 2;
        corner_1to2_shift = w;
        corner_1to3_shift = h * integral_block_histogram[0]->cols;
        corner_1to4_shift = corner_1to3_shift + w;
        // cache it now
        rule.cached_w = w;
        rule.cached_h = h;
        rule.cached_corner_1to2_shift = corner_1to2_shift;
        rule.cached_corner_1to3_shift = corner_1to3_shift;
        rule.cached_corner_1to4_shift = corner_1to4_shift;
        rule.cached = true;
    }
    else
    {
        w = rule.cached_w;
        h = rule.cached_h;
        corner_1to2_shift = rule.cached_corner_1to2_shift;
        corner_1to3_shift = rule.cached_corner_1to3_shift;
        corner_1to4_shift = rule.cached_corner_1to4_shift;
    }

    /**
     * Feature vector size gonna depend on rule.type
     * every call to 'localhistintegralsub' will add 'channels' elements
     * to the feature vector.
     */
    feature.resize(0);

    /**
     * in any case fill the first cells of the feature
     */
    localhistintegralsub(integral_block_histogram,          // <- input
                         channels,                          // <- input
                         corner_1,                          // <- input
                         corner_1 + corner_1to2_shift,      // <- input
                         corner_1 + corner_1to3_shift,      // <- input
                         corner_1 + corner_1to4_shift,      // <- input
                         normflag,                          // <- input
                         feature);                          // <- output

    /**
     * now depending on the rule.htype add some more information to the feature
     */
    if (rule.type == SrtTwoCols)
    {
        /** |-----|-----|
         *  |     |     |
         *  |  *  |  +  |
         *  |     |     |
         *  |-----|-----|
         */
        corner_1 += w;
        localhistintegralsub(integral_block_histogram,          // <- input
                             channels,                          // <- input
                             corner_1,                          // <- input
                             corner_1 + corner_1to2_shift,      // <- input
                             corner_1 + corner_1to3_shift,      // <- input
                             corner_1 + corner_1to4_shift,      // <- input
                             normflag,                          // <- input
                             feature);                          // <- output
        assert( feature.size() == 2 * channels );
    }
    else if (rule.type == SrtTwoRows)
    {
        /** |---------|
         *  |    *    |
         *  |---------|
         *  |    +    |
         *  |---------|
         */
        corner_1 += corner_1to3_shift;
        localhistintegralsub(integral_block_histogram,          // <- input
                             channels,                          // <- input
                             corner_1,                          // <- input
                             corner_1 + corner_1to2_shift,      // <- input
                             corner_1 + corner_1to3_shift,      // <- input
                             corner_1 + corner_1to4_shift,      // <- input
                             normflag,                          // <- input
                             feature);                          // <- output
        assert( feature.size() == 2 * channels );
    }
    else if (rule.type == SrtFourQuarters)
    {
        /** |-----|-----|
         *  |  *  |  +  |
         *  |-----|-----|
         *  |  +  |  +  |
         *  |-----|-----|
         */
        corner_1 += w;
        localhistintegralsub(integral_block_histogram,          // <- input
                             channels,                          // <- input
                             corner_1,                          // <- input
                             corner_1 + corner_1to2_shift,      // <- input
                             corner_1 + corner_1to3_shift,      // <- input
                             corner_1 + corner_1to4_shift,      // <- input
                             normflag,                          // <- input
                             feature);                          // <- output
        corner_1 += (corner_1to3_shift - w);
        localhistintegralsub(integral_block_histogram,          // <- input
                             channels,                          // <- input
                             corner_1,                          // <- input
                             corner_1 + corner_1to2_shift,      // <- input
                             corner_1 + corner_1to3_shift,      // <- input
                             corner_1 + corner_1to4_shift,      // <- input
                             normflag,                          // <- input
                             feature);                          // <- output
        corner_1 += w;
        localhistintegralsub(integral_block_histogram,          // <- input
                             channels,                          // <- input
                             corner_1,                          // <- input
                             corner_1 + corner_1to2_shift,      // <- input
                             corner_1 + corner_1to3_shift,      // <- input
                             corner_1 + corner_1to4_shift,      // <- input
                             normflag,                          // <- input
                             feature);                          // <- output
        assert( feature.size() == 4 * channels );
    }
    else
    {
        assert( rule.type == SrtOneSquare );
    }
}
